#ifndef CONTROLLER_H
#define	CONTROLLER_H

#include <iostream>
#include <string>
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
#include <geometry_msgs/Twist.h>
#include <boost/math/special_functions/fpclassify.hpp> // isnan
//#include <ros/console.h>
#include <vector>
#include <BonzoSurvivors/Beams.h>
#include "ObstacleAvoidanceBehav.h"
#include "AttractorBehav.h"
#include <geometry_msgs/Point.h>
#include <nav_msgs/Odometry.h>
#include <gripper_driver/Messaggio.h>

#define ALZA 1
#define ABBASSA 2
#define FERMA 3
#define ABBASSA_PALLA 6

enum PossibleStates
{
    WANDERING,
    FOUND,
    NEAR,
    ROTATING,
    FORKING,
    LIFT
};

class Controller {
public:
  Controller();
  Controller(const Controller& orig);
  virtual ~Controller();
  
  void execute();
private:
  
  float calcDistance();
  bool targetInFront();
  bool isForkDown();
  bool isForkUp();
  void newLaserScan(const sensor_msgs::LaserScan& scan_msg);
  void newBallCenter(const geometry_msgs::Point& circle_msg);
  void newOdometry(const nav_msgs::Odometry& odom_msg);
  
  double histogramOA(float aMin, float aMax);
  
  bool check(float r1, float r2, float th);

  int aToRange(float a);
  
  ros::NodeHandle nh_;
  ros::Subscriber scan_sub_,circle_sub_,odom_sub_;
  ros::ServiceClient client_;
  nav_msgs::Odometry odom_;
  nav_msgs::Odometry::_pose_type::_pose_type::_orientation_type initOrientation_;
  nav_msgs::Odometry::_pose_type::_pose_type::_position_type initPosition_;
  sensor_msgs::LaserScan scan_;
  geometry_msgs::Point ballCenter_;
  ros::Publisher vel_pub_,openglPub;
  geometry_msgs::Twist cmd;
  BonzoSurvivors::Beams beams_msg;
  bool flipped;
  bool pallaAlzata_;
  bool targetFound_;
  bool targetDetectionActivate_;
  bool obstacleAvoidance_;
  PossibleStates status_;
  
  double limit_;
  double DOOR;
  double defaultLinear_;
  double defaultTurn_;
  double linear_;
  double directionOA_;
  float nearDistance_;
  float centerTollerance_;
  int accuFront_;
  int accuNear_;
};

#endif	/* CONTROLLER_H */

